#include <boost/asio.hpp>
#include "boost_opt_util.h"
#include "trace.h"
#include "libpfcmu/capture++.h"
#include "libpfcmu/linux_aio.h"
#include "libpfcmu/util.h"

const unsigned int FRAMEDELAY = 1000;
const unsigned int FRAMECOUNTUNIT = 10000;
const unsigned int TOTALCAMS = 24;

inline timestamp_t getStartTS(timestamp_t start)
{
    timestamp_t result = (start+FRAMEDELAY)/FRAMECOUNTUNIT*FRAMECOUNTUNIT+FRAMECOUNTUNIT;
    return result;
}
int main(int argc, char *argv[])
{
    boost::program_options::options_description cmdline("Command line options");
    cmdline.add_options()
            ("help,h","show help message")
            ("port,p",
             boost::program_options::value<unsigned int>()->default_value(10086),
             "Main TCP connection port. For network commands.")
            ;
    boost::program_options::variables_map parameter_map = boost_opt_check(cmdline, argc, argv);

    const unsigned int CMDPORT = parameter_map["port"].as<unsigned int>();
    char hostname[32];
    gethostname(hostname, sizeof(hostname));

    try
    {
        const unsigned int C_RINGNUM = 8;
        const unsigned int D_RINGNUM = 8;
        using namespace boost::asio::ip;
        boost::asio::io_service mainIos;
        tcp::acceptor mainAcceptor(mainIos, tcp::endpoint(tcp::v4(), CMDPORT));
        TRACE(1, "%s: Main Server is Ready on Port %d.\n", hostname, CMDPORT);
        PFCMU::Capture capture;
        for(;;)
        {
            /*Init stage*/
            tcp::iostream cmdStream;
            mainAcceptor.accept(*cmdStream.rdbuf());
            bool m_IsCapturing = false;
            bool m_IsRunning = false;
            TRACE(1, "%s: Connection accepted.\n", hostname);
            cmdStream << std::string("REDY") << std::endl;
            std::string mainRecvCmd;
            double initShutter, initGain;
            unsigned int initFps;

            timestamp_t ts_prev = 0;
            std::string initLine;
            std::getline(cmdStream,initLine);
            std::istringstream initIss(initLine);
            initIss >> mainRecvCmd;

            if(mainRecvCmd=="ENPF")
            {
                initIss >> initFps >> initShutter >> initGain;
                TRACE(1, "%s: Start PF command received. FPS = %d, Shutter = %f, Gain = %f\n",
                      hostname, initFps, initShutter, initGain);
                capture.init(0, initFps);
                capture.start(C_RINGNUM);
                TRACE(1,"%s: ProFUSION start\n", hostname);

                capture.set_shutter(initShutter);
                capture.set_gain(initGain);
                TRACE(1,"%s: Shutter set to %f, Gain set to %f.\n", hostname, initShutter, initGain);
                TRACE(1, "%s: ProFUSION is running\n", hostname);
                m_IsRunning = true;
                //kill old frames
                for(unsigned int i=0;i<C_RINGNUM;i++)
                {
                    PF_EZImage *rbsImg = capture.grab();
                    const timestamp_t ts = capture.get_framecount();
                    ts_prev = ts;
                }
                cmdStream << "PFRY " << initFps << std::endl;
                TRACE(1, "%s: ProFUSION ready message sended.\n", hostname);
            }
            else
            {
                continue;
            }

            const unsigned int MEMSIZE = capture.width()*capture.height();
            for(std::string line;std::getline(cmdStream,line);)
            {
                timestamp_t startts = 0;
                unsigned int frames = 0;
                std::string fname;
                timestamp_t realstart = 0;

                if(line.empty() || line[0]=='\r')
                {
                    continue;
                }
                std::istringstream iss(line);
                std::string cmdHeader;
                iss >> cmdHeader;
                if(cmdHeader=="ENLV")
                {
                    //live
                    int watchingIdx = 0;
                    iss >> watchingIdx;
                    PF_EZImage *pfimg = capture.grab();
                    timestamp_t ts_curr = capture.get_framecount();
                    capture.embed_framecount();
                    for(unsigned int i=0;i<TOTALCAMS;i++)
                    {
                        cmdStream << "FULL\n"
                                  <<capture.width()<<"\n"
                                 <<capture.height()<<"\n"
                                <<i<<"\n"
                                  <<std::flush;
                        cmdStream.write(reinterpret_cast<const char *>(pfimg->imageArray[i]), MEMSIZE);
                    }
                }//ENLV
                else if(cmdHeader=="ENCP" && !m_IsCapturing)
                {
                    //start capture
                    PF_EZImage *currImg = capture.grab();
                    timestamp_t ts_curr = capture.get_framecount();
                    ts_prev = ts_curr;
                    capture.embed_framecount();
                    iss >> startts >> frames >> fname;
                    TRACE(1, "%s: Start Capture command received.\n", hostname);
                    if(startts==0)
                    {
                        realstart = getStartTS(ts_curr);
                    }
                    else
                    {
                        realstart = getStartTS(startts);
                    }
                    TRACE(1, "%s: Capture will start on %llu. Now is %llu\n", hostname, realstart, ts_curr);
                    m_IsCapturing = true;
                    PFCMU::libaio::writer_t writer;
                    writer.init(fname.c_str(), capture.memsize(), D_RINGNUM,frames,4096);
                    unsigned int m_CaptureCount = 0;
                    unsigned int m_FrameError = 0;
                    while(m_IsCapturing)
                    {
                        //capture
                        capture.grab();
                        timestamp_t ts_curr = capture.get_framecount();
                        capture.embed_framecount();
                        if(writer.is_initialized())
                        {
                            if(ts_curr < realstart)
                            {
                                m_CaptureCount = 0;
                            }
                            else if(m_CaptureCount<frames)
                            {
                                int id = writer.get_available_slot_id();
                                capture.copy_all(writer.buf(id));
                                writer.write(id, m_CaptureCount);
                                if(ts_curr-ts_prev>4)
                                {
                                    m_FrameError++;
                                    TRACE(1,"%s: Fream dropped between %llu and %llu\n", hostname, ts_prev, ts_curr);
                                }
                                m_CaptureCount++;
#ifdef DEBUGMODE
                                TRACE(1, "%s: Capturing frame %llu, %d of %d frames.\n",
                                      hostname, ts_curr, m_CaptureCount+1, frames);
#endif
                            }
                            else if(m_CaptureCount >= frames)
                            {
                                TRACE(1, "%s: Capture finished on frame %llu. Dropped %d Frames.\n",
                                      hostname, ts_curr, m_FrameError);
                                m_IsCapturing = false;
                                cmdStream << "CPFN " << ts_curr << " " << fname <<std::endl;
                            }
                        }//if(writer.is_initialized())
                        ts_prev = ts_curr;
                    }//while(m_IsCapturing)
                }//ENCP
                else if(cmdHeader=="STST")
                {
                    //set shutter
                }
                else if(cmdHeader=="STGN")
                {
                    //set gain
                }


            }//for(std::string line;std::getline(cmdStream,line);)



            capture.stop();
        }
    }
    catch(std::exception &e)
    {
        std::cerr << e.what() << std::endl;
    }

    return 0;
}


